//1.包含头文件
#include"mcu_comm/mcu_comm.hpp"

using namespace std;
commSerial* commserial;

std::ofstream outfile_rxdata;

MyTestMcuNode::MyTestMcuNode() : Node("Mcu_Comm")
{
  RCLCPP_INFO(this->get_logger(),"节点Mcu_Comm创建");
  mcu_comm_pub = this->create_publisher<base_interface::msg::McuComm>("/Slave_Data",10);

  // 接收控制命令，发给下位机
  control_cmd_sub = this->create_subscription<base_interface::msg::ControlCmd>("/cmd_vel",10,std::bind(&MyTestMcuNode::ControlCallback,this,std::placeholders::_1));
  hit_cmd_sub = this->create_subscription<base_interface::msg::Hit>("/cmd_hit",10,std::bind(&MyTestMcuNode::HitControlCallback ,this,std::placeholders::_1));
  timer_ = this->create_wall_timer(5ms,std::bind(&MyTestMcuNode::TimeCallback,this));
 
  time_t currentTime;
  time(&currentTime);
  currentTime = currentTime + 8 * 3600;           //	格林尼治标准时间+8个小时
  tm* t = gmtime(&currentTime);

  std::string file_out =  "data/rxdata/outfile_rxdata"+ to_string(t->tm_mon + 1) + "-" + to_string(t->tm_mday) + "." + to_string(t->tm_hour) + ":" + to_string(t->tm_min) + ".txt";
  outfile_rxdata.open(file_out.c_str(), ios::out | ios::trunc);
  outfile_rxdata.precision(5);
  outfile_rxdata.setf(ios_base::showpoint);
  
}


uint32_t ii;
double second_;

//灯板颜色： 0(灭) 1(白) 2(红) 3(绿) 4(蓝) 5(黄) 6(青) 7(粉) 8(橙) 9(青) 10(紫)
uint32_t light_state = 1;
void MyTestMcuNode::TimeCallback()
{
  ii++;
  second_ = ii*0.005;

  // if(second_ >= 3.0 )
  //   light_state = 4;
  // cout<<"time_now:"<<second_ << "          light_state: " << light_state <<endl;

  RxCallback();//收下位机的数
  ToSlaveCommunicate();//发给下位机数据

}


//----------------------------------------------------------------------------------------------------------
Vector2d Wheels_to_vehicle_speed(float v1, float v2, float v3, float v4)
{
  float vx,vy,v_com,v_angle;
  Vector2d Vcombine;
  vx = 0.5 * cos(PI/4)*(-v1-v2+v3+v4);
  vy = 0.5 * sin(PI/4)*( v1-v2-v3+v4);
  v_com = sqrt( pow(vx ,2) + pow(vy ,2) );
  v_angle = atan2(vy, vx)/PI*180;
  Vcombine << v_com, v_angle;
  return Vcombine;
}
//----------------------------------------------------------------------------------------------------------

//上位机 <-下位机
void MyTestMcuNode::RxCallback()
{
  // cout<<"in RxCallback"<<endl;
  
  auto rx_msg = base_interface::msg::McuComm();

  Rx_MCU mcuData;

  int a = commserial->commSerialRead();
  if(a > 0)
  {
    for(int i =0 ; i < RX_DATA_LEN; ++i)
    {
      if(commserial->rxbuf[0] == 'A' && commserial->rxbuf[1] =='T' && commserial->rxbuf[RX_DATA_LEN - 2]== 't' && commserial->rxbuf[RX_DATA_LEN - 1] == 'a')
      {
        mcuData.dataC[i] = commserial->rxbuf[i + 2];
      }
      else
      {
        return;
      }
    }

    //收到的四轮速度
    rx_msg.num[0]  = mcuData.dataF[0]*2*PI*0.08;//Cubemars velocity motor1 线速度 0.08m是轮子的半径
    rx_msg.num[1]  = mcuData.dataF[1]*2*PI*0.08;//Cubemars velocity motor2 
    rx_msg.num[2]  = mcuData.dataF[2]*2*PI*0.08;//Cubemars velocity motor3
    rx_msg.num[3]  = mcuData.dataF[3]*2*PI*0.08;//Cubemars velocity motor4
    
    //收到的击球机构位置信息
    rx_msg.num[4]  = mcuData.dataF[4];//击球机构的俯仰位置
    rx_msg.num[5]  = mcuData.dataF[5];//击球机构的发球位置

    rx_msg.num[6]  = mcuData.dataF[6] / 4096.f * 360.f;//上层机构俯仰位置
    rx_msg.num[7]  = mcuData.dataF[7] / 4096.f * 360.f;//上层机构横滚位置
    rx_msg.num[8]  = mcuData.dataF[8] / 4096.f * 360.f;//上层机构偏航位置
    rx_msg.num[9]  = mcuData.dataF[9];//4096.f/6.f;//上层机构三角梅位置
    rx_msg.num[10] = mcuData.dataF[10];///4096.f/6.f;//上层机构三角梅速度

    //flags
    rx_msg.num[11] = mcuData.dataF[11];//起垫开始flag
    rx_msg.num[12] = mcuData.dataF[12];//颠球完成flag
    rx_msg.num[13] = mcuData.dataF[13];//usb_flag
    rx_msg.num[14] = mcuData.dataF[14];

outfile_rxdata<<"起垫开始flag:\t"<<mcuData.dataF[11]<<"\t颠球完成flag\t"<<mcuData.dataF[12]<<endl;
    // rx_msg.time_stamp.stamp = this->now();

    // Vector2d Vcombine_; //＆速度的大小和方向
    // Vcombine_ = Wheels_to_vehicle_speed(rx_msg.num[0], rx_msg.num[1], rx_msg.num[2], rx_msg.num[3]);



    // RCLCPP_INFO(this->get_logger(), "下位机->上位机:%.1f, %.1f, %.1f, %.1f, ", mcuData.dataF[11],mcuData.dataF[12],mcuData.dataF[13],mcuData.dataF[14]);
    // RCLCPP_INFO(this->get_logger(), "下位机->上位机 灯板state:%.1f, ", mcuData.dataF[11]);
    // RCLCPP_INFO(this->get_logger(), "下位机->上位机 pitch::%.2f, roll:%.2f , yaw:%.2f, 三角梅位置:%.2f",rx_msg.num[6],rx_msg.num[7],rx_msg.num[8], rx_msg.num[9]);

    // RCLCPP_INFO(this->get_logger(), "轮速->车速=%.3f, angle=%.3f, motor1 = %.2f, motor2 = %.2f, motor3 = %.2f, motor4 = %.2f, date = %i",  Vcombine_(0),  Vcombine_(1), rx_msg.num[0], rx_msg.num[1], rx_msg.num[2], rx_msg.num[3],rx_msg.time_stamp.stamp.sec);
    // outfile_rxdata<<"motors_vel" <<"\t"<< mcuData.dataF[0]<<"\t"<<mcuData.dataF[1]<< "\t" <<mcuData.dataF[2] << "\t" <<mcuData.dataF[3]<< "\t" <<mcuData.dataF[4]<< "\t" <<mcuData.dataF[5]<< "\t" <<mcuData.dataF[6]<< "\t" <<mcuData.dataF[7]<< "\t" <<mcuData.dataF[8]<< "\t" <<mcuData.dataF[9]<< "\t" <<mcuData.dataF[10]<< "\t" <<mcuData.dataF[11] <<endl;
  //  cout<<"三角梅位置："<<mcuData.dataF[9]<<endl;
    // outfile_rxdata<<"位置\t" << rx_msg.num[9]<<"\t速度\t"<<rx_msg.num[10]<<endl;

  
  // cout<<"三角梅位置\t"<<rx_msg.num[9] <<"\t起垫开始\t" << rx_msg.num[11] <<"\t颠球完成flag\t"<<rx_msg.num[12]<<"\tusb_flag\t"<<rx_msg.num[13]<<endl;
  // outfile_rxdata<<"三角梅位置\t"<<rx_msg.num[9] <<"\t起垫开始\t" << rx_msg.num[11] <<"\t颠球完成flag\t"<<rx_msg.num[12]<<"\tusb_flag\t"<<rx_msg.num[13]<<endl;
  //  outfile_rxdata<<"三角梅电机位置\t"<< <<"\t三角梅的电机速度\t"<<<<endl;
    // outfile_rxdata<<"轮速->车速" <<"\t"<< Vcombine_(0) <<"\t"<< Vcombine_(1) <<"\t"<< "四轮速度：" <<"\t"<< rx_msg.num[0]<<"\t"<<rx_msg.num[1]<< "\t" <<rx_msg.num[2] << "\t" <<rx_msg.num[3]<<endl;

    mcu_comm_pub->publish(rx_msg);
  }
  
}

//上位机->下位机
void MyTestMcuNode::ToSlaveCommunicate()
{
  // cout<<"ToSlaveCommunicating"<<endl;

  Tx_MCU tx_data;
  commserial->txbuf[0] = 'A';
  commserial->txbuf[1] = 'T';
  commserial->txbuf[TX_DATA_LEN - 2] = 't';
  commserial->txbuf[TX_DATA_LEN - 1] = 'a';

  //底盘4个电机
  // tx_data.daraF[0] = CubeMar_Moter_one ;
  // tx_data.daraF[1] = CubeMar_Moter_two ;
  // tx_data.daraF[2] = CubeMar_Moter_thr ;
  // tx_data.daraF[3] = CubeMar_Moter_fou ;

    tx_data.daraF[0] = 0.0;
    tx_data.daraF[1] = 0.0;
    tx_data.daraF[2] = 0.0;
    tx_data.daraF[3] = 0.0;


  // printf("motor1-4: %.2f, %.2f, %.2f, %.2f\n",CubeMar_Moter_one,CubeMar_Moter_two ,CubeMar_Moter_thr,CubeMar_Moter_fou);
  printf("motor1-4: %.2f, %.2f, %.2f, %.2f\n", tx_data.daraF[0],tx_data.daraF[1],tx_data.daraF[2],tx_data.daraF[3]);

  //发球两个电机
  tx_data.daraF[4]  = 0;
  tx_data.daraF[5]  = 0;

  //颠球机构4个电机
  tx_data.daraF[6]  = hit_target_pitch_motor;//pitch俯仰(角度位置)
  tx_data.daraF[7]  = hit_target_roll_motor;//roll横滚(角度位置)
  tx_data.daraF[8]  = 0.0;//yaw偏航(角度位置)
  tx_data.daraF[9]  = hit_vel_motor;//三角梅速度(速度)

  //以下是flag
  tx_data.daraF[10] = 10.1f;  //function_chosen() 功能选择flag
  tx_data.daraF[11] = light_state;  
  tx_data.daraF[12] = hit_mcu_flag;//上位机给下位机发的击球flag
  tx_data.daraF[13] = 13.4f;
  tx_data.daraF[14] = 14.5f;
  tx_data.daraF[15] = 15.6f;

  for(int i = 0; i < TX_MCU_LENGTH; i++)
  {
    commserial->txbuf[i + 2] = tx_data.dataC[i];
  }
  commserial->commSerialWrite();
}




int in_control_callback = 0;
/**
 * @brief 底盘电机回调函数
*/
void MyTestMcuNode::ControlCallback(const base_interface::msg::ControlCmd::SharedPtr cmd_vel_)
{
//   in_control_callback++;
// cout<<"in_control_callback: "<<in_control_callback<<endl;
//   Tx_MCU tx_data;
//   commserial->txbuf[0] = 'A';
//   commserial->txbuf[1] = 'T'; 
//   commserial->txbuf[TX_DATA_LEN - 2] = 't';
//   commserial->txbuf[TX_DATA_LEN - 1] = 'a';

  CubeMar_Moter_one = cmd_vel_->target_one_motor_vel;
  CubeMar_Moter_two = cmd_vel_->target_two_motor_vel;
  CubeMar_Moter_thr = cmd_vel_->target_thr_motor_vel;
  CubeMar_Moter_fou = cmd_vel_->target_fou_motor_vel;
    
//   //发球两个电机
//   tx_data.daraF[4]  = 0;
//   tx_data.daraF[5]  = 0;

//   //颠球机构4个电机
//   tx_data.daraF[6]  = hit_target_pitch_motor;//pitch俯仰(角度位置)
//   tx_data.daraF[7]  = hit_target_roll_motor;//roll横滚(角度位置)
//   tx_data.daraF[8]  = 0;//yaw偏航(角度位置)
//   tx_data.daraF[9]  = hit_vel_motor;//三角梅速度(速度)

//   //以下是flag
//   tx_data.daraF[10] = 10.1f;  //function_chosen() 功能选择flag
//   tx_data.daraF[11] = light_state;  
//   tx_data.daraF[12] = hit_mcu_flag;//上位机给下位机发的击球flag
//   tx_data.daraF[13] = 13.4f;
//   tx_data.daraF[14] = 14.5f;
//   tx_data.daraF[15] = 15.6f;


//   for(int i = 0; i < TX_MCU_LENGTH; i++)
//   {
//     commserial->txbuf[i + 2] = tx_data.dataC[i];
//   }
//   commserial->commSerialWrite();

}

/**
 * @brief 垫球机构电机回调函数
*/
void MyTestMcuNode::HitControlCallback(const base_interface::msg::Hit::SharedPtr hit_msg)
{
   
  // std::cout<<"mcu_comm/HitControlCallback"<<std::endl;

  // const base_interface::msg::ControlCmd::ConstPtr hit_msg
  hit_target_pitch_motor  = hit_msg->target_pitch_motor;
  hit_target_roll_motor = hit_msg->target_roll_motor;
  hit_target_yaw_motor = hit_msg->target_yaw_motor;
  hit_vel_motor = hit_msg->hit_vel;
  hit_mcu_flag = hit_msg->hit_mcu_flag;

}


commSerial::commSerial(const char* port, uint32_t baudrat)
{
  std::cout<<"1\n";
  this_port = port;
  this_baudrate = baudrat;
}

commSerial::~commSerial(){}

void commSerial::commSerialInit()
{
  handle = open( this_port, O_RDWR | O_NOCTTY | O_NDELAY);
  std::cout << "handle:" << handle <<std::endl;

  while(handle ==-1)
  {
    handle = open( this_port, O_RDWR | O_NOCTTY | O_NDELAY);
    perror("open_port: Unable to open serial\n");
    std::cout << "handle = " << handle << std::endl;
  }
  tcgetattr(handle, &termAttr);
  cfsetispeed(&termAttr, this_baudrate);
  cfsetospeed(&termAttr, this_baudrate);
  termAttr.c_cflag &= ~PARENB;
  termAttr.c_cflag &= ~CSTOPB;
  termAttr.c_cflag &= ~CSIZE;
  termAttr.c_cflag |= CS8;
  termAttr.c_cflag |= (CLOCAL | CREAD);
  termAttr.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
  termAttr.c_iflag &= ~(IXON | IXOFF | IXANY);
  termAttr.c_oflag &= ~OPOST;
  termAttr.c_cc[VMIN] = 0;
  termAttr.c_cc[VTIME] = 1;
  tcflush(handle, TCIOFLUSH);
  tcsetattr(handle, TCSANOW, &termAttr);
  std::cout << "serial configured....\n";
}

int commSerial::commSerialRead()
{
  int MCU_len = 0;
  MCU_len = read(handle, rxbuf, RX_DATA_LEN);
  
  // if(MCU_len > 0)
  // {
  //   std::cout << "MCU_len:" << MCU_len << std::endl;
  // }
  return MCU_len;
}

// void commSerial::commSerialWrite(char* data)
// {
//   int len2 = write(handle, txbuf, TX_DATA_LEN);
//   if(len2 > 0)
//   {
//     std::cout << "len2:" << len2 << std::endl;
//   }
// }

void commSerial::commSerialWrite(void)
{
  int len2 = write(handle, txbuf, TX_DATA_LEN);
  // if(len2 > 0)
  // {
  //   std::cout << "len2:" << len2 << std::endl;
  // }
}


int main(int argc, char ** argv)
{

  auto vel_msg = base_interface::msg::ControlCmd();

//2.初始化ros2
  rclcpp::init(argc,argv);

  std::cout << "embedded init .." << std::endl;
  std::cout << "waiting for usb ..." << std::endl;
  commserial = new commSerial("/dev/dhrobot_base", B115200);// TODO need to change path  Bus 001 Device 008: ID
  //commserial = new commSerial("/dev/ttyUSB0", B115200);// TODO need to change path  Bus 001 Device 008: ID
  commserial->commSerialInit();
  std::cout << "init done !!!" <<std::endl;

//3.定义节点
  auto node = std::make_shared<MyTestMcuNode>();

//4.调用spin函数，并传入节点对象指针
  while(rclcpp::ok()){
    rclcpp::spin_some(node);
    
    // node->ToSlaveCommunicate();


    // node->RxCallback();

  }

//5.释放资源 
  rclcpp::shutdown();
  return 0;
}
